﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using OpenCvSharp;
using System.IO;
using ExpreRec.Logic.Preprocessing;
using ExpreRec.Logic.Util;

namespace ExpreRec.Logic.Expression
{
    public class GaborExtract
    {

        /// <summary>
        /// Normalize v with mean = 0, and deviation = 1
        /// </summary>
        /// <param name="v">Input array</param>
        /// <param name="start">Start point</param>
        /// <param name="length">Length</param>
        private void zeroMeanUnitLength(double[] v, int start, int length)
        {
            double sqsum = 0.0;
            double mean = v.Average(); //MeanVector(v,vSize);

            if (start + length > v.Length)
            {
                length = v.Length - start;
            }

            for (int i = start; i < start + length; i++)
            {
                v[i] -= mean;
                sqsum += v[i] * v[i];
            }
            double a = 1.0f / (double)(Math.Sqrt(sqsum));
            for (int i = start; i < start + length; i++)
            {
                v[i] *= a;
            }
        }

        /// <summary>
        /// Load gabor fiter bank from a folder
        /// </summary>
        /// <returns>40 CvMat</returns>
        private CvMat[] loadGaborFFT()
        {
            if (!File.Exists(string.Format("{0}\\FFTgabor_{1}_{2}.data", Const.GaborBaseFolder, 0, 0)))
                PreProcess();

            CvMat[] mGabor = new CvMat[Const.GaborScale * Const.GaborOrientation];

            for (int i = 0; i < Const.GaborScale; i++)
                for (int j = 0; j < Const.GaborOrientation; j++)
                {
                    string fname = string.Format("{0}\\FFTgabor_{1}_{2}.data", Const.GaborBaseFolder, i, j);
                    mGabor[i * Const.GaborOrientation + j] = BinaryFileHelper.GetMat2(fname);
                }

            return mGabor;
        }

        /// <summary>
        /// Convert Mat to Image
        /// </summary>
        /// <param name="mat">Mat</param>
        /// <returns>Image</returns>
        private IplImage convertMatToImage(CvMat mat)
        {
            double maxVal, minVal;

            Cv.MinMaxLoc(mat, out minVal, out maxVal);
            CvSize imsize = new CvSize(mat.Rows, mat.Cols);

            IplImage tmp = Cv.GetImage(mat);
            IplImage ipl = new IplImage(imsize, BitDepth.U8, 1);
            tmp.ConvertScale(ipl, 255.0 / (maxVal - minVal), -minVal * 255.0 / (maxVal - minVal));

            return ipl;
        }

        /// <summary>
        /// perform multiplication of two FFT factor in OPENCV
        /// </summary>
        /// <param name="srcAarr">input matrix 1</param>
        /// <param name="srcBarr">input matrix 2</param>
        /// <param name="dstarr">result matrix</param>
        private void mulFFT3(CvArr srcAarr, CvArr srcBarr, CvArr dstarr)
        {
            CvMat srcA = (CvMat)srcAarr;
            CvMat srcB = (CvMat)srcBarr;
            CvMat dst = (CvMat)dstarr;

            for (int i = 0; i < srcA.Rows; i++)
            {
                for (int j = 0; j < srcA.Cols; j++)
                {

                    double c_re = (srcA.DataArrayDouble[srcA.Width * 2 * i + j * 2]) * (srcB.DataArrayDouble[srcB.Width * 2 * i + j * 2]) -
                                  (srcA.DataArrayDouble[srcA.Width * 2 * i + j * 2 + 1]) * (srcB.DataArrayDouble[srcB.Width * 2 * i + j * 2 + 1]);

                    double c_im = (srcA.DataArrayDouble[srcA.Width * 2 * i + j * 2]) * (srcB.DataArrayDouble[srcB.Width * 2 * i + j * 2 + 1]) +
                                  (srcA.DataArrayDouble[srcA.Width * 2 * i + j * 2 + 1]) * (srcB.DataArrayDouble[srcB.Width * 2 * i + j * 2]);

                    dst.DataArrayDouble[dst.Width * 2 * i + j * 2] = c_re;
                    dst.DataArrayDouble[dst.Width * 2 * i + j * 2 + 1] = c_im;

                }
            }
        }

        /// <summary>
        /// perform gabor wavlet transform on the input image
        /// </summary>
        /// <param name="img">input image</param>
        /// <param name="objectf">output features</param>
        /// <param name="mGabor">Gabor filter</param>
        private void gaborExtraction(IplImage img, double[] objectf, CvMat[] mGabor, out IplImage[] filteredImages)
        {
            int w = Const.GaborMaskSize;
            int h = Const.GaborMaskSize;

            int dscale = Const.GaborDScale;

            CvMat imdft = new CvMat(w * 2, h * 2, MatrixType.F64C2);
            imdft.SetZero();

            for (int i = 0; i < h; i++)
                for (int j = 0; j < w; j++)
                {
                    imdft.DataArrayDouble[imdft.Width * 2 * i + j * 2] = img[i, j];
                    imdft.DataArrayDouble[imdft.Width * 2 * i + j * 2 + 1] = 0.0;
                }

            Cv.DFT(imdft, imdft, DFTFlag.Forward);

            int n = w * h / (dscale * dscale);

            IList<IplImage> res = new List<IplImage>();

            for (int i = 0; i < Const.GaborScale; i++)
            {
                for (int j = 0; j < Const.GaborOrientation; j++)
                {
                    CvMat gout = new CvMat(w * 2, h * 2, MatrixType.F64C2);

                    mulFFT3(imdft, mGabor[i * Const.GaborOrientation + j], gout);
                    Cv.DFT(gout, gout, DFTFlag.Inverse);

                    /*downsample sacle factor dscale*dscale*/
                    for (int x = dscale / 2; x < w; x += dscale)
                        for (int y = dscale / 2; y < h; y += dscale)
                        {
                            double sum = (gout.DataArrayDouble[gout.Width * 2 * (x + h / 2) + (y + w / 2) * 2]) *
                             (gout.DataArrayDouble[gout.Width * 2 * (x + h / 2) + (y + w / 2) * 2]) +
                             (gout.DataArrayDouble[gout.Width * 2 * (x + h / 2) + (y + w / 2) * 2 + 1]) *
                             (gout.DataArrayDouble[gout.Width * 2 * (x + h / 2) + (y + w / 2) * 2 + 1]);

                            objectf[(i * dscale + j) * n + x / dscale * h / dscale + y / dscale] = Math.Sqrt(sum);
                        }

                    //for testing

                    CvMat gimg = new CvMat(w, h, MatrixType.F64C1);
                    for (int x = 0; x < w; x++)
                        for (int y = 0; y < h; y++)
                        {
                            gimg.DataArrayDouble[gimg.Width * x + y] = Math.Sqrt((gout.DataArrayDouble[gout.Width * 2 * (x + h / 2) + (y + w / 2) * 2]) *
                                        (gout.DataArrayDouble[gout.Width * 2 * (x + h / 2) + (y + w / 2) * 2]) +
                                        (gout.DataArrayDouble[gout.Width * 2 * (x + h / 2) + (y + w / 2) * 2 + 1]) *
                                        (gout.DataArrayDouble[gout.Width * 2 * (x + h / 2) + (y + w / 2) * 2 + 1]));
                        }

                    res.Add(convertMatToImage(gimg));

                    //for testing
                    //Cv.ReleaseMat(gout);
                    zeroMeanUnitLength(objectf, (i * dscale + j) * n, n);
                }
            }
            //Cv.ReleaseMat(imdft);
            filteredImages = res.ToArray<IplImage>();
        }


        /// <summary>
        /// Extract various the features as large kinds of features as possible
        /// </summary>
        /// <param name="img">face image to be extracted</param>
        /// <returns>extracted feature vector</returns>
        public double[] ExtractFeatures(IplImage img, out IplImage[] filteredImages)
        {
            int nsize = Const.GaborOrientation * Const.GaborScale * Const.GaborMaskSize * Const.GaborMaskSize / (Const.GaborDScale * Const.GaborDScale);

            CvSize dstSize = new CvSize(Const.GaborMaskSize, Const.GaborMaskSize);

            IplImage ipl;
            if (img.NChannels == 3)
                ipl = Processor.RGBToGray(img);
            else ipl = img.Clone();

            if ((dstSize.Width != img.Width) || (dstSize.Height != img.Height))
                ipl = Processor.ReSize(ipl, dstSize, Interpolation.Linear);

            double[] objectf = new double[nsize];

            IplImage tmp = new IplImage(dstSize, BitDepth.F64, 0);
            Cv.ConvertScale(ipl, tmp, 1.0, 0);

            CvMat[] mGabor = loadGaborFFT();
            /*Gabor wavelet*/
            gaborExtraction(tmp, objectf, mGabor, out filteredImages);

            zeroMeanUnitLength(objectf, 0, nsize);
            //Cv.ReleaseImage(tmp);
            //Cv.ReleaseImage(ipl);

            //unloadGaborFFT(mGabor);

            return objectf;
        }

        /// <summary>
        /// Gabor Wavelet Transform filter generator,image features extraction. 
        /// centered = 0;
        /// Should it generate the filter with 0,0 at the ll corner or the center normalized = 1; 
        /// Should the filters be normalized? 
        /// @return filter is saved in the file.
        /// </summary>
        /// <param name="kmax">kmax = 1.57f;	Positive Pi/2 for the KMax variable for the highest frequency with the smallest kernel size </param>
        /// <param name="sigma">sigma = 6.28f;	Positive 2Pi for the sigma variable for every frequency </param>
        private void gaborKernel(double kmax, double sigma)
        {
            double[] gaborCos = new double[Const.GaborMaskSize * Const.GaborMaskSize];
            double[] gaborSin = new double[Const.GaborMaskSize * Const.GaborMaskSize];
            int offset = Const.GaborMaskSize / 2;

            double f = Math.Sqrt(2.0);
            double sig = sigma * sigma;
            for (int i = 0; i < Const.GaborScale; i++)
                for (int j = 0; j < Const.GaborOrientation; j++)
                {
                    double kv = kmax / Math.Pow(f, i);
                    double phiu = j * Math.PI / 8.0;
                    double kv_mag = kv * kv;

                    for (int x = 0; x < Const.GaborMaskSize; x++)
                        for (int y = 0; y < Const.GaborMaskSize; y++)
                        {
                            int xo = x - offset;
                            int yo = y - offset;
                            double mag = (double)(xo * xo + yo * yo);
                            gaborCos[x * Const.GaborMaskSize + y] = kv_mag / sig * Math.Exp(-0.5 * kv_mag * mag / sig) *
                                    (Math.Cos(kv * (xo * Math.Cos(phiu) + yo * Math.Sin(phiu))) - Math.Exp(-1.0 * sig / 2.0));

                            gaborSin[x * Const.GaborMaskSize + y] = kv_mag / sig * Math.Exp(-0.5 * kv_mag * mag / sig) *
                                    (Math.Sin(kv * (xo * Math.Cos(phiu) + yo * Math.Sin(phiu))));//-exp(-1.0*sig/2.0)
                        }

                    string cname = string.Format("{0}\\Cgabor_{1}_{2}.data", Const.GaborBaseFolder, i, j);
                    string sname = string.Format("{0}\\Sgabor_{1}_{2}.data", Const.GaborBaseFolder, i, j);
                    string fname = string.Format("{0}\\FFTgabor_{1}_{2}.data", Const.GaborBaseFolder, i, j);

                    string cnameImg = string.Format("{0}\\Cgabor_{1}_{2}.bmp", Const.GaborBaseFolder, i, j);
                    string snameImg = string.Format("{0}\\Sgabor_{1}_{2}.bmp", Const.GaborBaseFolder, i, j);


                    /* Let's deal with the cosine part first */

                    CvMat cmat = new CvMat(Const.GaborMaskSize, Const.GaborMaskSize, MatrixType.F64C1, gaborCos);
                    BinaryFileHelper.WriteMat(cmat, cname);
                    convertMatToImage(cmat).SaveImage(cnameImg);

                    /* Now let's deal with the sine part */
                    CvMat smat = new CvMat(Const.GaborMaskSize, Const.GaborMaskSize, MatrixType.F64C1, gaborSin);
                    BinaryFileHelper.WriteMat(smat, sname);
                    convertMatToImage(cmat).SaveImage(snameImg);
                }
        }

        /// <summary>
        /// Save pre-computed fft of gabor filter
        /// </summary>
        public void PreProcess()
        {
            if (!Directory.Exists(Const.GaborBaseFolder))
                Directory.CreateDirectory(Const.GaborBaseFolder);

            gaborKernel(Math.PI / 2.0, 2 * Math.PI);

            int dft_M = Cv.GetOptimalDFTSize(Const.GaborMaskSize + Const.GaborMaskSize - 1);
            int dft_N = Cv.GetOptimalDFTSize(Const.GaborMaskSize + Const.GaborMaskSize - 1);
            for (int i = 0; i < Const.GaborScale; i++)
            {
                for (int j = 0; j < Const.GaborOrientation; j++)
                {
                    string cname = string.Format("{0}\\Cgabor_{1}_{2}.data", Const.GaborBaseFolder, i, j);
                    string sname = string.Format("{0}\\Sgabor_{1}_{2}.data", Const.GaborBaseFolder, i, j);
                    string fname = string.Format("{0}\\FFTgabor_{1}_{2}.data", Const.GaborBaseFolder, i, j);

                    CvMat mGaborReal = BinaryFileHelper.GetMat(cname);
                    CvMat mGaborImg = BinaryFileHelper.GetMat(sname);

                    CvMat mGabor = new CvMat(dft_M, dft_N, MatrixType.F64C2);
                    CvMat mGaborFFT = new CvMat(dft_M, dft_N, MatrixType.F64C2);
                    Cv.SetZero(mGabor);

                    for (int x = 0; x < Const.GaborMaskSize; x++)
                        for (int y = 0; y < Const.GaborMaskSize; y++)
                        {
                            mGabor.DataArrayDouble[mGabor.Width * x * 2 + y * 2] =
                            mGaborReal.DataArrayDouble[mGaborReal.Width * x + y];
                            mGabor.DataArrayDouble[mGabor.Width * x * 2 + y * 2 + 1] =
                            mGaborImg.DataArrayDouble[mGaborImg.Width * x + y];
                        }

                    Cv.DFT(mGabor, mGaborFFT, DFTFlag.Forward, Const.GaborMaskSize);


                    BinaryFileHelper.WriteMat2(mGaborFFT, fname);

                    //Cv.ReleaseMat(mGaborFFT);
                    //Cv.ReleaseMat(mGabor);
                    //Cv.ReleaseMat(mGaborReal);
                    //Cv.ReleaseMat(mGaborImg);
                }
            }
        }
    }
}